#include "myMain.h"

/* 配置 Config */
#define SHOOT_ENABLE			1				//推射使能
#define DRIBBLE_ENABLE			1				//运球使能
#define SHOOT_SET_ORIGIN_ENABLE	1				//推射上电设置原点使能
#define ORIGIN_CHEAK_CAN_ENABLE	0				//初始化时候是否检查几秒CAN连接

#if !SHOOT_ENABLE								//自动禁用运球
	#undef DRIBBLE_ENABLE
	#define DRIBBLE_ENABLE		0
#endif

#define ENC_SPEED_ENABLE    	0 				//是否定时计算编码器速度

#define NORMAL 					sShootCMD.Duty 	
#define MODE10 					10
#define MODE50					50
#define MODE95					95
#define DEBUGMODE 				NORMAL			//调试模式

#define cShootAngle				2150.f			//推射角度 2150.f

#define cCatchAngle				800.f			//运球接球的位置
#define cShootDefenseAngle		1500.f			//一分钟准备初始化的时候电机停的位置
#define cDeathZone				3.f				//死区

#define cDJISpeed				30.f
#define cDJISpeedL				(-cDJISpeed)
#define cDJISpeedR				cDJISpeed
#define cDJISpeedDutyLimit		30
#define cDJICurrentDutyLimit	30
#define cClawCurrentDutyLimit	50

#define cEscaladeClawAngle		700.f					//云梯抓球位置
#define cEscaladeClawAngleL		cEscaladeClawAngle
#define cEscaladeClawAngleR		(-cEscaladeClawAngle)

#define cEscaladeShootWaitAngle		600.f			//云梯射球位置
#define cEscaladeShootWaitAngleL	cEscaladeShootWaitAngle
#define cEscaladeShootWaitAngleR	(-cEscaladeShootWaitAngle)

#define cEscaladeButtonAngle		980.f			
#define cEscaladeButtonAngleL		cEscaladeButtonAngle
#define cEscaladeButtonAngleR		(-cEscaladeButtonAngle)

#define cEscaladeDribbleAngle	30.f					//云梯运球位置
#define cEscaladeDribbleAngleL	cEscaladeDribbleAngle
#define cEscaladeDribbleAngleR	(-cEscaladeDribbleAngle)

#define cEscaladeNoDribbleAngle		30.f					//  爪子坏了云梯不运球等待的位置
#define cEscaladeNoDribbleAngleL	cEscaladeNoDribbleAngle
#define cEscaladeNoDribbleAngleR	(-cEscaladeNoDribbleAngle)

/***************/


#include "AS5047P.h"

/* CAN数量定义 **********/

#define VESCNum 5
#define DMNum 	4
#define DJINum 	4
#define CANNum (VESCNum + DMNum + DJINum)

/* 电机数量定义↑↑↑↑↑*/

extern VESC_HandleTypeDef WheelF;     //轮电机
extern VESC_HandleTypeDef WheelL;
extern VESC_HandleTypeDef WheelB;
extern VESC_HandleTypeDef WheelR;
extern DJI_HandleTypeDef HelmF;        //舵电机
extern DJI_HandleTypeDef HelmL;
extern DJI_HandleTypeDef HelmB;
extern DJI_HandleTypeDef HelmR;
#define WheelFID 42
#define WheelBID 44
#define WheelLID 41
#define WheelRID 43
#define HelmFID  2
#define HelmBID  4
#define HelmLID  1
#define HelmRID  3

extern VESC_HandleTypeDef Shoot;     //推射电机
#define ShootID	50

extern DJI_HandleTypeDef EscaladeL;
extern DJI_HandleTypeDef EscaladeR;
extern DJI_HandleTypeDef ClawL;
extern DJI_HandleTypeDef ClawR;
#define EscaladeLID 1
#define EscaladeRID 2
#define ClawLID     3
#define ClawRID     4


extern uint8_t ClawLRxFlag;
extern uint8_t ClawRRxFlag;
extern uint8_t EscaladeLFlag;
extern uint8_t EscaladeRFlag;

extern DJI_HandleTypeDef Defense;
#define DefenseID   5


extern uint16_t IDArray[CANNum];
extern uint8_t IDFlagArray[CANNum];

/* 推射相关变量 */
extern float gShootNowAngle;

/* 编码器相关参数定义 */

extern float gShootLastAngle;
extern float gShootReadAngle;
extern float gShootNowAngle_buff;

extern AS5047P_HandleTypeDef shootEnc;
extern WS2811_HandleTypeDef LEDStrip;

/* 离合刹车宏定义 */
#define clutchUnBreak() 		HAL_GPIO_WritePin(shootBreak_GPIO_Port, shootBreak_Pin, GPIO_PIN_RESET) 	/* 离合器取消刹车 */
#define clutchBreak() 			HAL_GPIO_WritePin(shootBreak_GPIO_Port, shootBreak_Pin, GPIO_PIN_SET);		/* 离合器刹车 */

#define cClawBigOpenAngle 		100.f
#define cClawBigOpenAngleL		(-(cClawOpenAngle))
#define cClawBigOpenAngleR		(cClawOpenAngle)
#define cClawOpenAngle 			100.f
#define cClawOpenAngleL			(-(cClawOpenAngle))
#define cClawOpenAngleR		    (cClawOpenAngle)
#define cClawCloseAngle 		37.f
#define cClawCloseAngleL 		(-(cClawCloseAngle))
#define cClawCloseAngleR 	    (cClawCloseAngle)
#define cClawHalfCloseAngle 	45.f
#define cClawHalfCloseAngleL 	(-(cClawHalfCloseAngle))
#define cClawHalfCloseAngleR 	(cClawHalfCloseAngle)
//张开爪子
#define openClaw(delay,delayTime)		ClawL.pf->posMode(&ClawL,cClawOpenAngleL,0,cClawCurrentDutyLimit);		\
										delay(delayTime);														\
										ClawR.pf->posMode(&ClawR,cClawOpenAngleR,0,cClawCurrentDutyLimit)
//大开爪子(运球的时候张大点)
#define bigOpenClaw(delay,delayTime)	ClawL.pf->posMode(&ClawL,cClawBigOpenAngleL,0,cClawCurrentDutyLimit);	\
										delay(delayTime);														\
										ClawR.pf->posMode(&ClawR,cClawBigOpenAngleR,0,cClawCurrentDutyLimit)
//合上爪子
#define closeClaw(delay,delayTime)		ClawL.pf->posMode(&ClawL,cClawCloseAngleL,0,cClawCurrentDutyLimit + 50);		\
										delay(delayTime);														\
										ClawR.pf->posMode(&ClawR,cClawCloseAngleR,0,cClawCurrentDutyLimit + 50)
//半合上爪子
#define closeHalfClaw(delay,delayTime)	ClawL.pf->posMode(&ClawL,cClawHalfCloseAngleL,0,cClawCurrentDutyLimit - 5);		\
										delay(delayTime);														\
										ClawR.pf->posMode(&ClawR,cClawHalfCloseAngleR,0,cClawCurrentDutyLimit - 5)
//合上爪子低电流
#define closeClawLowCurrent(delay,delayTime)		ClawL.pf->posMode(&ClawL,cClawCloseAngleL,0,cClawCurrentDutyLimit / 2);		\
													delay(delayTime);														\
													ClawR.pf->posMode(&ClawR,cClawCloseAngleR,0,cClawCurrentDutyLimit / 2)
//半合上爪子低电流
#define closeHalfClawLowCurrent(delay,delayTime)	ClawL.pf->posMode(&ClawL,cClawHalfCloseAngleL,0,cClawCurrentDutyLimit / 2);		\
													delay(delayTime);														\
													ClawR.pf->posMode(&ClawR,cClawHalfCloseAngleR,0,cClawCurrentDutyLimit / 2)

#define pushCylinder()	HAL_GPIO_WritePin(cylinder_GPIO_Port, cylinder_Pin, GPIO_PIN_SET)
#define pullCylinder()	HAL_GPIO_WritePin(cylinder_GPIO_Port, cylinder_Pin, GPIO_PIN_RESET)

#define pushShootCylinder() HAL_GPIO_WritePin(shootCylinder_GPIO_Port, shootCylinder_Pin, GPIO_PIN_SET)
#define pullShootCylinder() HAL_GPIO_WritePin(shootCylinder_GPIO_Port, shootCylinder_Pin, GPIO_PIN_RESET)

#if ENC_SPEED_ENABLE
extern volatile float gShootSpeed;
#endif

void myOsDelay(uint32_t myDelayTime);
BaseType_t myQueueSend(QueueHandle_t queue, void *itemToSend, uint32_t delayTime);


extern volatile struct myErrState gErrState;
extern volatile bool gMotorRxInitFinishFlag;
extern volatile bool gMechInitFinishAndStartEncFlag;
extern volatile bool gMechInitFinishAndInFreeRTOSFlag;
extern volatile bool gMotorONFlag;
extern volatile bool gIfDrible;
extern volatile uint8_t gMechInitMode;

void deviceInit();
void mechInit();
void communicateInit();

